/* -*- Mode: C; indent-tabs-mode: t; c-basic-offset: 4; tab-width: 4 -*- */
/*
 * bocciawii
 * Copyright (C) Grupo de trabajo ULE 2010 <ulewii@googlegroups.com>
 * 
 * bocciawii is free software: you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by the
 * Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 * 
 * bocciawii is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
 * See the GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License along
 * with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#include "elemento-dinamico.h"
ElementoDinamico::ElementoDinamico(int dureza, std::string equipo)
						:ElementoEstacionario(0)
{
	
	for (int i = 0; i < 3; i++)
	{
		velocidad_lineal[i] = 0;
		velocidad_angular[i] = 0;
	}
	velocidad_escalar_horizontal = 0;
	velocidad_escalar_vertical = 0;

	_equipo = equipo;
	_dureza = dureza;
	_bolaLanzada = 0;

	provoca_colision = 0;

	// Asignar propiedades físicas
	switch (_dureza)
	{
		case DUREZA_BLANDA:
			coeficiente = COEFICIENTE_BLANDA;
			restitucion = RESTITUCION_BLANDA;
			break;
		case DUREZA_DURA:
			coeficiente = COEFICIENTE_DURA;
			restitucion = RESTITUCION_DURA;
			break;
		case DUREZA_SEMI:
			coeficiente = COEFICIENTE_MEDIA;
			restitucion = RESTITUCION_MEDIA;
			break;
	}

	tiempo_acumulado = 0;
}


void ElementoDinamico::Init(float velocidad, float angulo)
{
	this->Vel(velocidad * cos(angulo * M_PI / 180), velocidad * sin(angulo * M_PI / 180));
	    
	tiempo_acumulado = 0;
	float t_subida = velocidad_escalar_vertical / GRAVEDAD;
	float altura_maxima = posicion[1] + (velocidad_escalar_vertical * t_subida) - (0.5 * GRAVEDAD * pow(t_subida, 2));
	float t_bajada = sqrt((2 * (altura_maxima - RADIO_BOLA)) / GRAVEDAD);

	tiempo_aire = t_subida + t_bajada;
	
	// Posición en el momento de tocar el suelo
	posicion_caida[0] = posicion[0] + velocidad_escalar_horizontal * velocidad_lineal[0] * tiempo_aire;
	posicion_caida[1] = RADIO_BOLA - coeficiente / 100; // restar segun dureza
	posicion_caida[2] = posicion[2] + velocidad_escalar_horizontal * velocidad_lineal[2] * tiempo_aire;
	/*std::cerr << "Datos iniciales" << std::endl;
	std::cerr << "Posicion: (" << posicion[0] << ", " << posicion[1] << ", " << posicion[2] << ")" << std::endl;
	std::cerr << "Velocidad vertical inicial: " << velocidad_escalar_vertical << std::endl;
	std::cerr << "Velocidad horizontal inicial: " << velocidad_escalar_horizontal << std::endl;
	std::cerr << "Tiempo en el aire: " << tiempo_aire << std::endl;*/
}


void ElementoDinamico::Dir (float x, float y, float z) 
{
	velocidad_lineal[0] = x;
	velocidad_lineal[1] = y;
	velocidad_lineal[2] = z;
}

void ElementoDinamico::Vel (float v_hor, float v_vert)
{
	velocidad_escalar_horizontal = v_hor;
	velocidad_escalar_vertical = v_vert;

	if (isnan(velocidad_escalar_horizontal))
		velocidad_escalar_horizontal = 0.4;
}

void ElementoDinamico::Ang (float rx, float ry, float rz)
{
	velocidad_angular[0] = rx;
	velocidad_angular[1] = ry;
	velocidad_angular[2] = rz;
}

void ElementoDinamico::VelIncremental (float x, float y, float z) 
{
	velocidad_lineal[0] += x;
	velocidad_lineal[1] += y;
	velocidad_lineal[2] += z;
}

void ElementoDinamico::AngIncremental (float rx, float ry, float rz)
{
	velocidad_angular[0] += rx;
	velocidad_angular[1] += ry;
	velocidad_angular[2] += rz;
}

void ElementoDinamico::ActualizarPosicion (float milis)
{
	tiempo_acumulado += milis / 1000;
	//std::cerr << "Tiempo acumulado en segundos: " << tiempo_acumulado << std::endl;

	

	if (tiempo_acumulado < tiempo_aire) 
	{
		
		posicion[1] += (velocidad_escalar_vertical * milis / 1000) - (0.5 * GRAVEDAD * pow(milis / 1000, 2));
		velocidad_escalar_vertical -= GRAVEDAD * milis / 1000;

		posicion[0] += velocidad_escalar_horizontal * velocidad_lineal[0] * milis / 1000;
		posicion[2] += velocidad_escalar_horizontal * velocidad_lineal[2] * milis / 1000;
	}
	else if (tiempo_acumulado - tiempo_aire < milis)
	{
		float tiempo_suelo = tiempo_acumulado - tiempo_aire;
		posicion[1] = RADIO_BOLA - coeficiente / 100; // restar un poco según la dureza
		velocidad_escalar_vertical = 0.0;

		posicion[0] += ((velocidad_escalar_horizontal * milis / 1000) 
			- (0.5 * velocidad_escalar_horizontal * coeficiente * GRAVEDAD * pow(milis / 1000, 2)))
			* velocidad_lineal[0];
		posicion[2] += ((velocidad_escalar_horizontal * milis / 1000) 
			- (0.5 * velocidad_escalar_horizontal * coeficiente * GRAVEDAD * pow(milis / 1000, 2)))
			* velocidad_lineal[2];
		velocidad_escalar_horizontal -= coeficiente * GRAVEDAD * milis / 1000;
		if (velocidad_escalar_horizontal < 0.005)
			velocidad_escalar_horizontal = 0.0;
		// El coeficiente debe incluir otras cosas como el peso, pero como éste es constante se puede meter ahí.
		// TODO hacer pruebas para ver cómo queda.
	}
	else
	{
		posicion[1] = RADIO_BOLA - coeficiente / 100; // restar un poco según la dureza
		velocidad_escalar_vertical = 0.0;

		posicion[0] += ((velocidad_escalar_horizontal * milis / 1000) 
			- (0.5 * velocidad_escalar_horizontal * coeficiente * GRAVEDAD * pow(milis / 1000, 2)))
			* velocidad_lineal[0];
		posicion[2] += ((velocidad_escalar_horizontal * milis / 1000) 
			- (0.5 * velocidad_escalar_horizontal * coeficiente * GRAVEDAD * pow(milis / 1000, 2)))
			* velocidad_lineal[2];		
		velocidad_escalar_horizontal -=  coeficiente * GRAVEDAD * milis / 1000;
		if (velocidad_escalar_horizontal < 0.005)
			velocidad_escalar_horizontal = 0.0;	
	}
	// std::cerr << "Velocidad horizontal " << velocidad_escalar_horizontal << std::endl;
	// std::cerr << "Bola en (" << posicion[0] << ", " << posicion[1] << ", " << posicion[2] << ")" << std::endl;
}

int ElementoDinamico::EnMovimiento()
{
	if ((velocidad_escalar_horizontal != 0) || (velocidad_escalar_vertical != 0))
		return 1;
	return 0;
}

float ElementoDinamico::Distancia(ElementoDinamico* el)
{
	float salida = 0;
	
	float* posicion_bola = el->GetPosicion();
	salida = sqrt(pow(posicion_bola[0] - posicion[0], 2) 
	    + pow(posicion_bola[1] - posicion[1], 2)
	    + pow(posicion_bola[2] - posicion[2], 2));

	return salida;	
}

int ElementoDinamico::DentroCancha()
{
	float x, z;
	x = posicion[0];
	z = posicion[2];

	// Descartamos los puntos que no requieren demasiada comprobacion
	if (x < LIMITE_OESTE || x > LIMITE_ESTE)
		return 0;
	if (z < LIMITE_NORTE || z > PUNTA_TRIANGULO_Y)
		return 0;

	// Parte de la cancha correspondiente al cuadrado del fondo
	if (z < BASE_TRIANGULO_Y)
		return 1;	

	float r0x, r0z, r1x, r1z, cos1, cos2; // r2x, r2z
	// Triangulo. Se calcula el ángulo y se compara	
	r0x = LIMITE_ESTE - PUNTA_TRIANGULO_X;
	r0z = 0;
	
	r1x = x - PUNTA_TRIANGULO_X;
	r1z = z - PUNTA_TRIANGULO_Y;

	//r2x = LIMITE_ESTE - PUNTA_TRIANGULO_X;
	//r2z = BASE_TRIANGULO_Y - PUNTA_TRIANGULO_Y;

	cos1 = fabs((r1x * r0x + r1z * r0z) / (sqrt(pow(r1x, 2) + pow(r1z, 2)) * sqrt(pow(r0x, 2) + pow(r0z, 2))));
	cos2 = 0.894427; //fabs((r2x * r0x + r2z * r0z) / (sqrt(pow(r2x, 2) + pow(r2z, 2)) * sqrt(pow(r0x, 2) + pow(r0z, 2))));

	//std::cout << "Coseno bola, coseno triangulo: " << cos1 << ", " << cos2 << std::endl;

	if (cos1 < cos2)
		return 1;

	return 0;
}